# 读取 nav_msgs/ 地图信息
from typing import Any
import rospy
from nav_msgs.msg import OccupancyGrid
import get_info_from_gz as gzinfo
import numpy as np
import threading
import time

class OccupancyGridReader():
    def __init__(self):
        self.currtent_map = OccupancyGrid()
        self.uav_info = gzinfo.GetInfoFromGz('uav1/')
        rospy.Subscriber('/map2d', OccupancyGrid, self.occupancy_grid_callback)
        
        self.lock = threading.Lock()
    # 回调函数
    def occupancy_grid_callback(self, msg):
        # 在回调函数中处理接收到的占用栅格地图消息
        self.currtent_map.header.stamp = msg.header.stamp
        self.currtent_map.header.frame_id = msg.header.frame_id
        self.currtent_map.info.resolution = msg.info.resolution
        self.currtent_map.info.origin.position.x = msg.info.origin.position.x
        self.currtent_map.info.origin.position.y = msg.info.origin.position.y
        self.currtent_map.info.origin.position.z = msg.info.origin.position.z
        self.currtent_map.info.width = msg.info.width
        self.currtent_map.info.height = msg.info.height
        self.currtent_map.data = msg.data
    
    def get_map(self):
        return self.currtent_map
    
    # 获取无人机真实位置
    def get_uav_real_position(self):
        uav_real_position = self.uav_info.get_gazebo_pose().pose.pose.position
        return uav_real_position
    
    # 获取无人机栅格地图坐标
    def get_uav_grid_position(self):
        uav_real_position = self.get_uav_real_position()
        # print(self.currtent_map.info)
        time.sleep(0.1)
        uav_grid_position = self.real_to_grid(uav_real_position)
        return uav_grid_position
    
    # 无人机真实位置与栅格地图坐标转换
    def real_to_grid(self, position):
        # 栅格地图坐标
        # print(self.currtent_map.info)
        time.sleep(0.1)
        grid_x = int((position.x - self.currtent_map.info.origin.position.x) / self.currtent_map.info.resolution)
        grid_y = int((position.y - self.currtent_map.info.origin.position.y) / self.currtent_map.info.resolution)

        return [grid_x, grid_y]
    
    # 栅格地图坐标与真实位置转换
    def grid_to_real(self, position):
        # 真实位置
        # 不sleep的话，会产生除0错误
        time.sleep(0.1)
        real_x = position[0] * self.currtent_map.info.resolution + self.currtent_map.info.origin.position.x
        real_y = position[1] * self.currtent_map.info.resolution + self.currtent_map.info.origin.position.y
        return [real_x, real_y]

if __name__ == '__main__':
    rospy.init_node('occupancy_grid_reader')
    occupancy_grid_reader = OccupancyGridReader()
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        # print(occupancy_grid_reader.currtent_map.info)
        print(occupancy_grid_reader.currtent_map.data)
        rate.sleep()